#ifndef _CORE_VEC3_H_
#define _CORE_VEC3_H_

template<class T>
class Vec3
{
	public:
		union {
			T field[3];
			struct {
				T x;
				T y;
				T z;
			};
		};

		Vec3();
		Vec3(T x, T y, T z);
		Vec3<T> normal();
		Vec3<T> rescale(T new_len);
		Vec3<T> clamp(T max_len);

        void normalize();
		void unfloat(T epsilon);

		T length();

		Vec3<T> operator*(T f);
		Vec3<T> operator+(const Vec3& b);
		Vec3<T> operator-(const Vec3& b);
		Vec3<T>& operator+=(const Vec3& v);

		T& operator[](int n);
};

template<class T>
Vec3<T>::Vec3()
{
	x = y = z = (T)0;
}

template<class T>
Vec3<T>::Vec3(T _x, T _y, T _z)
{
	x=_x; y=_y; z=_z;
}

template<class T>
Vec3<T> Vec3<T>::operator*(T f)
{
	return Vec3(x*f,y*f,z*f);
}

template<class T>
Vec3<T> Vec3<T>::operator+(const Vec3& b)
{
	return Vec3(x+b.x,y+b.y,z+b.z);
}

template<class T>
Vec3<T>& Vec3<T>::operator+=(const Vec3& v)
{
	*this=*this+v;
	return *this;
}

template<class T>
Vec3<T> Vec3<T>::operator-(const Vec3& b)
{
	return Vec3(x-b.x,y-b.y,z-b.z);
}

template<class T>
T& Vec3<T>::operator[](int n)
{
	if (n<0 || n>2) throw "Invalid vector dimension requested";
	return field[n];
}

template<class T>
Vec3<T> Vec3<T>::normal()
{
	Vec3 result = *this;
	result.normalize();
	return result;
}

template<class T>
T Vec3<T>::length()
{
	return (T) sqrt(x*x+y*y+z*z);
}

template<class T>
Vec3<T> Vec3<T>::rescale(T new_len)
{
	T len = length();

	if (len==0)
		return Vec3();
	T mod = new_len / len;
	x *= mod;
	y *= mod;
	z *= mod;

}

template<class T>
void Vec3<T>::normalize()
{
	T len=length();
	if (len==0) len=1;
	x/=len;
	y/=len;
	z/=len;
}

template<class T>
Vec3<T> Vec3<T>::clamp(T max_len)
{
	Vec3 foo = *this;

	T len = length();

	if (len > max_len)
		return rescale(max_len);

	return foo;
}

template<class T>
void Vec3<T>::unfloat(T epsilon)
{
	for (int i=0; i<3; i++)
		if (fabs(field[i]) < epsilon) field[i] = 0;
}

#endif // _CORE_VEC3_H_

